/*
initialStageAction.hpp
*/
#include "behaviortree_cpp/action_node.h"
#include "utils/load_data.hpp"

using namespace std::chrono_literals;
class InitialStage : public BT::StatefulActionNode
{
    typedef struct
    {
        int zero;           //峰值前的零点
        int peak;
        int aft_zero;       //峰值的零点
    }PeakZeroPair;

    typedef struct
    {
        uint64_t tick;
        double euler;
        double gyr;
        double distance;
    }DataInfo;
    
    public:
    explicit InitialStage(const std::string &name,  const BT::NodeConfig& config) : BT::StatefulActionNode(name, config){cnt_ = 0;};
        BT::NodeStatus onStart() override;

        BT::NodeStatus onRunning() override; 
            void onHalted() override{
                //处理中断
            std::cout << "InitialStage_onHalted()" << std::endl;
            }
            // 添加静态端口声明方法
            static BT::PortsList providedPorts() {
                return BT::PortsList({
                    BT::InputPort<sensorData>("sensor_input", "Description of input"),
                    BT::InputPort<uint64_t>("duration", "3","Description of input"),
                    BT::OutputPort<uint64_t>("time_stamp", "Description of output"),
                    BT::OutputPort<uint64_t>("T_to", "Description of input"),
                    BT::OutputPort<uint64_t>("T_hs", "The latest T_hs timestamp"),
                    BT::OutputPort<std::shared_ptr<std::vector<uint64_t>>>("init_mean_swing_duration", "Description of input"),
                    BT::OutputPort<std::shared_ptr<std::vector<uint64_t>>>("init_mean_stance_duration", "Description of input"),
                    BT::OutputPort<std::shared_ptr<std::vector<uint64_t>>>("init_mean_T_hs_potential_region", "Description of input"),
                });
            }
    private:
            int cnt_;
            sensorData tmp_sensor_data_filtered_;
            bool time_flag_ = 1;
            uint64_t start_millis_=0;
            uint64_t current_millis_=0;
            int start_pos_ = 0;              //数组起始位置
            void func_findPeaks(void);
            void Upsampling(void);
            std::vector<DataInfo> buffer_;  // 缓存：每行 11 列数据
            std::vector<double> row_data_;//将端口数据转换为按行存储
            std::vector<uint64_t> T_to_;
            std::vector<uint64_t> T_hs_;
            std::vector<uint64_t> mean_swing_duration_;
            std::vector<uint64_t> mean_stance_duration_ = {};
            std::vector<uint64_t> mean_T_hs_potential_region_={};
            std::vector<PeakZeroPair> peak_pos_list_;                 //每一步的峰值
            
            double laser_ranging_max_;
            double laser_ranging_min_;
            double laser_ranging_sum_;
            double maximum_threshold_;
            std::vector<double> filter_buffer_;
            std::shared_ptr<std::vector<uint64_t>> mean_stance_duration_ptr = nullptr;
            std::shared_ptr<std::vector<uint64_t>> mean_swing_duration_ptr = nullptr;
            uint64_t T_to_last_ = 0;
            uint64_t T_hs_last_ = 0;

        //top数据归一化处理
        void filter_laser_ranging(std::vector<DataInfo> &data_buffer, int start);
        //寻找duration_ms时段内每一步的峰值
        int find_top_distance_peak(std::vector<double> &filter_buffer, double maximum_threshold, double duration_ms);
        //寻找距离峰值最近的零点位置
        int find_nearest_zero_point(std::vector<double> &filter_buffer);
        uint64_t find_T_hs_event(std::vector<DataInfo> &data_buffer, int start, int t_to, uint64_t region_ms, double slot_ms);
        void print_all_outports(bool print_switch);
        void print_all_inports(bool print_switch);
        void print_vector(const std::vector<uint64_t>& vec, const std::string& name);
        void print_var(const uint64_t& var, const std::string& name);
}; // 确保这里有分号


// ANSI 颜色代码
#define COLOR_RED     "\033[31m"
#define COLOR_GREEN   "\033[32m"
#define COLOR_YELLOW  "\033[33m"
#define COLOR_BLUE    "\033[34m"
#define COLOR_MAGENTA "\033[35m"
#define COLOR_CYAN    "\033[36m"
#define COLOR_RESET   "\033[0m"    

